
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       battery_monitor_backend.c
  * @author     baiyang
  * @date       2022-10-26
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "battery_monitor_backend.h"

#include <string.h>

#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void bind_function(battery_monitor_backend_t battmonitor_backend, battery_monitor_backend_ops_t ops);
static bool capacity_remaining_pct(battery_monitor_backend_t battmonitor_backend, uint8_t *percentage);
static enum BattMonitorFailsafe update_failsafes(battery_monitor_backend_t battmonitor_backend);
static bool reset_remaining(battery_monitor_backend_t battmonitor_backend, float percentage);
static bool update_check(size_t buflen, char *buffer, bool failed, const char *message);
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// we declare a virtual destructor so that BattMonitor driver can
// override with a custom destructor if need be
void battmonitor_backend_ctor(battery_monitor_backend_t battmonitor_backend,
                                           battery_monitor_backend_ops_t ops,
                                           battery_monitor_state_t mon_state,
                                           battery_monitor_params_t params)
{
    bind_function(battmonitor_backend, ops);

    battmonitor_backend->_state = mon_state;
    battmonitor_backend->_params = params;
}

/// get voltage with sag removed (based on battery current draw and resistance)
/// this will always be greater than or equal to the raw voltage
float battmonitor_backend_voltage_resting_estimate(battery_monitor_backend_t battmonitor_backend)
{
    // resting voltage should always be greater than or equal to the raw voltage
    return MAX(battmonitor_backend->_state->voltage, battmonitor_backend->_state->voltage_resting_estimate);
}

// update battery resistance estimate and voltage_resting_estimate
void battmonitor_backend_update_resistance_estimate(battery_monitor_backend_t battmonitor_backend)
{
    // return immediately if no current
    if (!battmonitor_backend_has_current(battmonitor_backend) || !math_flt_positive(battmonitor_backend->_state->current_amps)) {
        return;
    }

    // update maximum current seen since startup and protect against divide by zero
    battmonitor_backend->_current_max_amps = MAX(battmonitor_backend->_current_max_amps, battmonitor_backend->_state->current_amps);
    float current_delta = battmonitor_backend->_state->current_amps - battmonitor_backend->_current_filt_amps;
    if (math_flt_zero(current_delta)) {
        return;
    }

    // update reference voltage and current
    if (battmonitor_backend->_state->voltage > battmonitor_backend->_resistance_voltage_ref) {
        battmonitor_backend->_resistance_voltage_ref = battmonitor_backend->_state->voltage;
        battmonitor_backend->_resistance_current_ref = battmonitor_backend->_state->current_amps;
    }

    // calculate time since last update
    uint32_t now = time_millis();
    float loop_interval = (now - battmonitor_backend->_resistance_timer_ms) * 0.001f;
    battmonitor_backend->_resistance_timer_ms = now;

    // estimate short-term resistance
    float filt_alpha = math_constrain_float(loop_interval/(loop_interval + MB_BATT_MONITOR_RES_EST_TC_1), 0.0f, 0.5f);
    float resistance_alpha = MIN(1, MB_BATT_MONITOR_RES_EST_TC_2*fabsf((battmonitor_backend->_state->current_amps-battmonitor_backend->_current_filt_amps)/battmonitor_backend->_current_max_amps));
    float resistance_estimate = -(battmonitor_backend->_state->voltage-battmonitor_backend->_voltage_filt)/current_delta;
    if (math_flt_positive(resistance_estimate)) {
        battmonitor_backend->_state->resistance = battmonitor_backend->_state->resistance*(1-resistance_alpha) + resistance_estimate*resistance_alpha;
    }

    // calculate maximum resistance
    if ((battmonitor_backend->_resistance_voltage_ref > battmonitor_backend->_state->voltage) && (battmonitor_backend->_state->current_amps > battmonitor_backend->_resistance_current_ref)) {
        float resistance_max = (battmonitor_backend->_resistance_voltage_ref - battmonitor_backend->_state->voltage) / (battmonitor_backend->_state->current_amps - battmonitor_backend->_resistance_current_ref);
        battmonitor_backend->_state->resistance = MIN(battmonitor_backend->_state->resistance, resistance_max);
    }

    // update the filtered voltage and currents
    battmonitor_backend->_voltage_filt = battmonitor_backend->_voltage_filt*(1-filt_alpha) + battmonitor_backend->_state->voltage*filt_alpha;
    battmonitor_backend->_current_filt_amps = battmonitor_backend->_current_filt_amps*(1-filt_alpha) + battmonitor_backend->_state->current_amps*filt_alpha;

    // update estimated voltage without sag
    battmonitor_backend->_state->voltage_resting_estimate = battmonitor_backend->_state->voltage + battmonitor_backend->_state->current_amps * battmonitor_backend->_state->resistance;
}

// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool battmonitor_backend_arming_checks(battery_monitor_backend_t battmonitor_backend, char *buffer, size_t buflen)
{
    bool low_voltage, low_capacity, critical_voltage, critical_capacity;
    battmonitor_backend_check_failsafe_types(battmonitor_backend, &low_voltage, &low_capacity, &critical_voltage, &critical_capacity);

    bool below_arming_voltage = math_flt_positive(battmonitor_backend->_params->_arming_minimum_voltage) &&
                                (battmonitor_backend->_state->voltage < battmonitor_backend->_params->_arming_minimum_voltage);
    bool below_arming_capacity = (battmonitor_backend->_params->_arming_minimum_capacity > 0) &&
                                 ((battmonitor_backend->_params->_pack_capacity - battmonitor_backend->_state->consumed_mah) < battmonitor_backend->_params->_arming_minimum_capacity);
    bool fs_capacity_inversion = math_flt_positive(battmonitor_backend->_params->_critical_capacity) &&
                                 math_flt_positive(battmonitor_backend->_params->_low_capacity) &&
                                 (battmonitor_backend->_params->_low_capacity < battmonitor_backend->_params->_critical_capacity);
    bool fs_voltage_inversion = math_flt_positive(battmonitor_backend->_params->_critical_voltage) &&
                                math_flt_positive(battmonitor_backend->_params->_low_voltage) &&
                                (battmonitor_backend->_params->_low_voltage < battmonitor_backend->_params->_critical_voltage);

    bool result = update_check(buflen, buffer, !battmonitor_backend->_state->healthy, "unhealthy");
    result = result && update_check(buflen, buffer, below_arming_voltage, "below minimum arming voltage");
    result = result && update_check(buflen, buffer, below_arming_capacity, "below minimum arming capacity");
    result = result && update_check(buflen, buffer, low_voltage,  "low voltage failsafe");
    result = result && update_check(buflen, buffer, low_capacity, "low capacity failsafe");
    result = result && update_check(buflen, buffer, critical_voltage, "critical voltage failsafe");
    result = result && update_check(buflen, buffer, critical_capacity, "critical capacity failsafe");
    result = result && update_check(buflen, buffer, fs_capacity_inversion, "capacity failsafe critical > low");
    result = result && update_check(buflen, buffer, fs_voltage_inversion, "voltage failsafe critical > low");

    return result;
}

// checks what failsafes could be triggered
void battmonitor_backend_check_failsafe_types(battery_monitor_backend_t battmonitor_backend,
                                                                 bool *low_voltage, bool *low_capacity,
                                                                 bool *critical_voltage,
                                                                 bool *critical_capacity)
{
    // use voltage or sag compensated voltage
    float voltage_used;
    switch (battery_monitor_params_failsafe_voltage_source(battmonitor_backend->_params)) {
        case BattMonitor_LowVoltageSource_Raw:
        default:
            voltage_used = battmonitor_backend->_state->voltage;
            break;
        case BattMonitor_LowVoltageSource_SagCompensated:
            voltage_used = battmonitor_backend_voltage_resting_estimate(battmonitor_backend);
            break;
    }

    // check critical battery levels
    if ((voltage_used > 0) && (battmonitor_backend->_params->_critical_voltage > 0) && (voltage_used < battmonitor_backend->_params->_critical_voltage)) {
        *critical_voltage = true;
    } else {
        *critical_voltage = false;
    }

    // check capacity failsafe if current monitoring is enabled
    if (battmonitor_backend_has_current(battmonitor_backend) && (battmonitor_backend->_params->_critical_capacity > 0) &&
        ((battmonitor_backend->_params->_pack_capacity - battmonitor_backend->_state->consumed_mah) < battmonitor_backend->_params->_critical_capacity)) {
        *critical_capacity = true;
    } else {
        *critical_capacity = false;
    }

    if ((voltage_used > 0) && (battmonitor_backend->_params->_low_voltage > 0) && (voltage_used < battmonitor_backend->_params->_low_voltage)) {
        *low_voltage = true;
    } else {
        *low_voltage = false;
    }

    // check capacity if current monitoring is enabled
    if (battmonitor_backend_has_current(battmonitor_backend) && (battmonitor_backend->_params->_low_capacity > 0) &&
        ((battmonitor_backend->_params->_pack_capacity - battmonitor_backend->_state->consumed_mah) < battmonitor_backend->_params->_low_capacity)) {
        *low_capacity = true;
    } else {
        *low_capacity = false;
    }
}

void battmonitor_backend_update_consumed(battery_monitor_state_t state, uint32_t dt_us)
{
    // update total current drawn since startup
    if (state->last_time_micros != 0 && dt_us < 2000000) {
        const float mah = battmonitor_backend_calculate_mah(state->current_amps, dt_us);
        state->consumed_mah += mah;
        state->consumed_wh  += 0.001 * mah * state->voltage;
    }
}

void Log_Write_BAT(battery_monitor_backend_t battmonitor_backend, const uint8_t instance, const uint64_t time_us) {}
void Log_Write_BCL(battery_monitor_backend_t battmonitor_backend, const uint8_t instance, const uint64_t time_us) {}

static void bind_function(battery_monitor_backend_t battmonitor_backend, battery_monitor_backend_ops_t ops)
{
    battmonitor_backend->ops = ops;

    if (battmonitor_backend->ops->capacity_remaining_pct == NULL) {
        battmonitor_backend->ops->capacity_remaining_pct = capacity_remaining_pct;
    }

    if (battmonitor_backend->ops->update_failsafes == NULL) {
        battmonitor_backend->ops->update_failsafes = update_failsafes;
    }

    if (battmonitor_backend->ops->reset_remaining == NULL) {
        battmonitor_backend->ops->reset_remaining = reset_remaining;
    }
}

// capacity_remaining_pct - returns true if the battery % is available and writes to the percentage argument
// return false if the battery is unhealthy, does not have current monitoring, or the pack_capacity is too small
static bool capacity_remaining_pct(battery_monitor_backend_t battmonitor_backend, uint8_t *percentage)
{
    // we consider anything under 10 mAh as being an invalid capacity and so will be our measurement of remaining capacity
    if (battmonitor_backend->_params->_pack_capacity <= 10) {
        return false;
    }

    // the monitor must have current readings in order to estimate consumed_mah and be healthy
    if (!battmonitor_backend_has_current(battmonitor_backend) || !battmonitor_backend->_state->healthy) {
        return false;
    }

    const float mah_remaining = battmonitor_backend->_params->_pack_capacity - battmonitor_backend->_state->consumed_mah;
    *percentage = math_constrain_float(100 * mah_remaining / battmonitor_backend->_params->_pack_capacity, 0, UINT8_MAX);
    return true;
}

static enum BattMonitorFailsafe update_failsafes(battery_monitor_backend_t battmonitor_backend)
{
    const uint32_t now = time_millis();

    bool low_voltage, low_capacity, critical_voltage, critical_capacity;
    battmonitor_backend_check_failsafe_types(battmonitor_backend, &low_voltage, &low_capacity, &critical_voltage, &critical_capacity);

    if (critical_voltage) {
        // this is the first time our voltage has dropped below minimum so start timer
        if (battmonitor_backend->_state->critical_voltage_start_ms == 0) {
            battmonitor_backend->_state->critical_voltage_start_ms = now;
        } else if (battmonitor_backend->_params->_low_voltage_timeout > 0 &&
                   now - battmonitor_backend->_state->critical_voltage_start_ms > (uint32_t)(battmonitor_backend->_params->_low_voltage_timeout)*1000U) {
            return BattMonitorFailsafeCritical;
        }
    } else {
        // acceptable voltage so reset timer
        battmonitor_backend->_state->critical_voltage_start_ms = 0;
    }

    if (critical_capacity) {
        return BattMonitorFailsafeCritical;
    }

    if (low_voltage) {
        // this is the first time our voltage has dropped below minimum so start timer
        if (battmonitor_backend->_state->low_voltage_start_ms == 0) {
            battmonitor_backend->_state->low_voltage_start_ms = now;
        } else if (battmonitor_backend->_params->_low_voltage_timeout > 0 &&
                   now - battmonitor_backend->_state->low_voltage_start_ms > (uint32_t)(battmonitor_backend->_params->_low_voltage_timeout)*1000U) {
            return BattMonitorFailsafeLow;
        }
    } else {
        // acceptable voltage so reset timer
        battmonitor_backend->_state->low_voltage_start_ms = 0;
    }

    if (low_capacity) {
        return BattMonitorFailsafeLow;
    }

    // if we've gotten this far then battery is ok
    return BattMonitorFailsafeNone;
}

/*
  default implementation for reset_remaining(). This sets consumed_wh
  and consumed_mah based on the given percentage. Use percentage=100
  for a full battery
*/
static bool reset_remaining(battery_monitor_backend_t battmonitor_backend, float percentage)
{
    percentage = math_constrain_float(percentage, 0, 100);
    const float used_proportion = (100.0f - percentage) * 0.01f;
    battmonitor_backend->_state->consumed_mah = used_proportion * battmonitor_backend->_params->_pack_capacity;
    // without knowing the history we can't do consumed_wh
    // accurately. Best estimate is based on current voltage. This
    // will be good when resetting the battery to a value close to
    // full charge
    battmonitor_backend->_state->consumed_wh = battmonitor_backend->_state->consumed_mah * 0.001f * battmonitor_backend->_state->voltage;

    // reset failsafe state for this backend
    battmonitor_backend->_state->failsafe = battmonitor_backend_update_failsafes(battmonitor_backend);

    return true;
}

static bool update_check(size_t buflen, char *buffer, bool failed, const char *message)
{
    if (failed) {
        strncpy(buffer, message, buflen);
        return false;
    }
    return true;
}
/*------------------------------------test------------------------------------*/


